﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Threading;
using RayDen.Library.Components;
using RayDen.Library.Components.Accelleration;
using RayDen.Library.Core;
using RayDen.Library.Core.Primitives;
using RayDen.Library.Entity.Scene;

namespace RayDen.SmallPT.Acceleration
{
    using BaseRenderable = Triangle;
    using RenderableCollection = List<Triangle>;


    public class BvhBuilder
    {
        private const int PrimitivesInNode = 1;
        private int costSamples = 4, isectCost = 80, traversalCost = 10;
        private float emptyBonus = 0.25f;

        public void BuildTree(List<Triangle> primitives, int start, int end)
        {
        }
    }

    public class BoundingVolumeHierarchyAccelerationStructure : IAccellerationStructure
    {
        private const int PrimitivesInNode = 4;
        private int costSamples = 4, isectCost = 80, traversalCost = 10;
        private float emptyBonus = 0.25f;
        private IMaterialProvider materialProvider;

        public Node RootNode;

        #region Internal Types

        public class Node
        {
            public AABB Bound;
            public Node Left, Right;
            public Axis SplitAxis;
            public int StartOffset, nPrims;
        }


        internal class BVHBuildNode
        {
            public AABB bounds;
            public BVHBuildNode[] children;
            public int splitAxis, firstOffset, nPrims;

            internal bool IsLeaf
            {
                get { return children[0] != null && children[1] != null; }
            }
            internal BVHBuildNode()
            {
                children = new BVHBuildNode[2];
            }
            internal void InitLeaf(int first, int nP, ref AABB box)
            {
                bounds = box;
                nPrims = nP;
                firstOffset = first;
            }
            internal void InitInterior(BVHBuildNode left, BVHBuildNode right, int axis)
            {
                children[0] = left;
                children[1] = right;
                splitAxis = axis;
                bounds = left.bounds.Union(right.bounds);
                nPrims = 0;
            }
        }


        internal class LinearBVHNode
        {
            public AABB bbox;//+12+12+4
            public int primOffset;//+4
            public int secondChildOffset;//+4
            public byte nPrimitives;//+1
            public byte axis;//+1
            public void SetChildOffset(int sc)
            {
                this.secondChildOffset = sc;
            }
            public void SetOffset(int offset)
            {
                this.primOffset = offset;
            }
        }

        internal struct BBox
        {
            public Point Min;
            public Point Max;

            public BBox(AABB b)
            {
                this.Max = b.Max;
                this.Min = b.Min;
            }

            public static bool IntersectBox(BBox box, RayInfo ray)
            {
                float t0 = ray.Min;
                float t1 = ray.Max;

                var rayInvDir = ray.InvDir;
                //1f /ray.Dir;

                float invDirX = rayInvDir.x;
                if (!process_box_coord(ref t0, ref t1, box.Min.x - ray.Org.x, box.Max.x - ray.Org.x, invDirX))
                    return false;

                float invDirY = rayInvDir.y;
                if (!process_box_coord(ref t0, ref t1, box.Min.y - ray.Org.y, box.Max.y - ray.Org.y, invDirY))
                    return false;

                float invDirZ = rayInvDir.z;
                if (!process_box_coord(ref t0, ref t1, box.Min.z - ray.Org.z, box.Max.z - ray.Org.z, invDirZ))
                    return false;
                return true;
            }
            private static bool process_box_coord(ref float t0, ref float t1, float mnRO, float mxRO, float invRayDir)
            {
                float tNear = (mnRO) * invRayDir;
                float tFar = (mxRO) * invRayDir;
                // Update parametric interval from slab intersection $t$s
                if (tNear > tFar)
                    swap(ref tNear, ref tFar);
                t0 = tNear > t0 ? tNear : t0;
                t1 = tFar < t1 ? tFar : t1;
                if (t0 > t1)
                    return false;
                return true;
            }
            private static void swap(ref float tNear, ref  float tFar)
            {
                float tmp = tNear;
                tNear = tFar;
                tFar = tmp;
            }
        }

        [StructLayout(LayoutKind.Explicit, Size = 32)]
        internal struct LBVHNode
        {
            [FieldOffset(0)]
            public BBox Bound;
            [FieldOffset(24)]
            public Int32 PrimOffset;
            [FieldOffset(24)]
            public Int32 SecondChildOffset;
            [FieldOffset(28)]
            public byte PrimitiveCount;
            [FieldOffset(29)]
            public byte Axis;
        }

        #endregion

        private Point[] sceneVertices;
        private Triangle[] triangles;
        private List<TriangleMesh> meshes;
        private int bvhNodesCount;

        public void BuildTree(List<Triangle> primitives, int start, int end)
        {


            Console.WriteLine("Building accelleration {0}  {1} Node size in bytes", primitives.Count, Marshal.SizeOf(typeof(LBVHNode)));

            if (start - end == 0)
                return;
            AABB bound = primitives.GetRange(start, end - start).Select(prim => prim.WorldBound(ref sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));


            RootNode = new Node() { Bound = bound, SplitAxis = (Axis)bound.MaximumExtent() };
            bvhNodesCount++;
            if ((end - start) <= PrimitivesInNode)
            {
                RootNode.StartOffset = start;
                RootNode.nPrims = end - start;
                return;
            }
            RootNode.Left = this.BuildSubTree(primitives, start, (start + end) / 2);
            RootNode.Right = this.BuildSubTree(primitives, (start + end) / 2, end);
        }

        private BVHBuildNode rootNode;

        public void BuildSAH()
        {

        }
        internal interface INodeComparer
        {
            bool Compare(Triangle p);
        }

        internal class MidComparer : INodeComparer
        {

            public int dim;
            public float pMid;
            public Point[] vertices;

            public bool Compare(Triangle p)
            {
                return p.Center(ref vertices)[dim] < pMid;
            }

        }
        private Node BuildSubTree(RenderableCollection primitives, int start, int end)
        {
            if (start - end == 0)
                return null;
            AABB bound = primitives.GetRange(start, end - start).Select(prim => prim.WorldBound(ref sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));
            var node = new Node() { Bound = bound, SplitAxis = (Axis)bound.MaximumExtent() };
            bvhNodesCount++;
            if ((end - start) <= PrimitivesInNode)
            {
                node.StartOffset = start;
                node.nPrims = end - start;
                //node.Primitives = primitives.GetRange(start, (end - start));
                return node;
            }
            

            //float pmid;
            int axis = bound.MaximumExtent();
            Comparison<Triangle> tc = (t1, t2) => CompareTriangles(t1, t2, (Axis)bound.MaximumExtent());

            primitives.PartialSort(tc, start, end);

            //FindBestSplit(primitives, start, end, out pmid, out axis);

            

            //var pmid = .5f * (bound.Min[axis] + bound.Max[axis]);
            //FindBestSplit(primitives.GetRange(start, end-start), start, end, out pmid, out axis);
            //var cmp = new MidComparer() { dim = axis, pMid = pmid, vertices = sceneVertices};
            node.SplitAxis = (Axis) axis;

            var mid =
                //start+Algorithms.Partition(primitives.GetRange(start, end - start), cmp.Compare);
                (start + end) / 2;
            node.Left = this.BuildSubTree(primitives, start, mid);
            node.Right = this.BuildSubTree(primitives, mid, end);
            return node;
        }

        private List<LinearBVHNode> nodes;
        private LBVHNode[] bvhTree;
        private int FlattenBVHTree(Node node, ref int offset)
        {
            if (node == null)
            {
                return -1;
            }
            nodes[offset] = new LinearBVHNode() { bbox = node.Bound };
            var linearNode = nodes[offset];
            int myOffset = (offset)++;
            if (node.nPrims > 0)
            {
                linearNode.primOffset = node.StartOffset;
                linearNode.nPrimitives = (byte)node.nPrims;
            }
            else
            {
                linearNode.axis = (byte)node.SplitAxis;
                linearNode.nPrimitives = 0;
                FlattenBVHTree(node.Left, ref offset);
                linearNode.secondChildOffset = FlattenBVHTree(node.Right, ref offset);
            }
            return myOffset;
        }

        private void FlattenTree()
        {
            int offset = 0;
            nodes = Enumerable.Range(0, bvhNodesCount).Select(i => new LinearBVHNode()).ToList();
            this.FlattenBVHTree(RootNode, ref offset);
            bvhTree = nodes.Select(linearBvhNode => linearBvhNode.nPrimitives > 0
                                                        ? new LBVHNode()
                                                        {
                                                            Axis = linearBvhNode.axis,
                                                            Bound = new BBox(linearBvhNode.bbox),
                                                            PrimOffset = linearBvhNode.primOffset,
                                                            PrimitiveCount = linearBvhNode.nPrimitives
                                                        }
                                                        : new LBVHNode()
                                                        {
                                                            Axis = linearBvhNode.axis,
                                                            Bound = new BBox(linearBvhNode.bbox),
                                                            PrimitiveCount = linearBvhNode.nPrimitives,
                                                            SecondChildOffset = linearBvhNode.secondChildOffset
                                                        }).ToArray();
            RootNode = null;
            nodes.Clear();
        }

        #region BVH Implementation

        public const int TraversalStackSize = 32;

        private bool IntersectBVH(RayInfo ray, ref IntersectionInfo sp)
        {
            if (sp == null)
            {
                sp = new IntersectionInfo();
            }
            if (bvhTree.Length == 0)
                return false;
            int[] todo = new int[TraversalStackSize];

            TriangleMesh lastHitMesh = null;
            int lastHitIndex = -1, triangleIndex = -1;
            float u = 0f, v = 0f;
            bool hit = false;
            Vector invDir = ray.InvDir;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            var dist = float.MaxValue;
            while (true)
            {
                var node = bvhTree[nodeNum];
                float t1 = 0f;
                if (BBox.IntersectBox(node.Bound, ray))
                {
                    //if (node.bbox.Intersect(vray, out t0, out t2)) {
                    if (node.PrimitiveCount > 0)
                    {
                        for (var i = 0; i < node.PrimitiveCount; ++i)
                        {
                            if (triangles[node.PrimOffset + i].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                            {
                                if (dist > t1)
                                {
                                    triangleIndex = node.PrimOffset + i;
                                    lastHitMesh = triangles[node.PrimOffset + i].Owner;
                                    lastHitIndex = triangles[node.PrimOffset + i].Index;
                                    dist = t1;
                                    sp.GeometryInfo.HitPoint = ray.Point(dist);
                                    sp.RayDist = dist;
                                    sp.PrimitiveIndex = node.PrimOffset + i;
                                    sp.PrimitiveId = triangles[node.PrimOffset + i].Owner.Id;
                                    //sp.Geometry = triangles[node.PrimOffset + i];
                                    //sp.Distance = dist;
                                    hit = true;
                                }
                            }
                        }
                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else
                    {
                        if (dirIsNeg[node.Axis])
                        {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = node.SecondChildOffset;
                        }
                        else
                        {
                            todo[todoOffset++] = node.SecondChildOffset;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else
                {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            if (hit)
            {
                sp.Material = materialProvider.Get(lastHitMesh.MaterialName);
                var gnormal = triangles[triangleIndex].InterpolateMeshNormal(u, v).Normalize();
                Normal shadeN;

                if (Vector.Dot(ref ray.Dir, ref gnormal) >= 0f)
                    shadeN = -gnormal;
                else
                    shadeN = gnormal;

                sp.GeometryInfo.ShadingNormal = shadeN;
                sp.GeometryInfo.GeoNormal = gnormal;
                //triangles[node.primOffset + i].InterpolateNormal(ref sceneModel.Vertices, u, v).Normalize();
                sp.GeometryInfo.UvPoint  = lastHitMesh.InterpolateTriUV(lastHitIndex, u, v);
                return true;
            }

            return false;
        }

        private bool ShadowIntersectBVH(RayInfo ray)
        {
            if (nodes.Count == 0)
                return false;
            bool hit = false;
            Vector invDir = ray.InvDir;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            int[] todo = new int[TraversalStackSize];
            while (true)
            {
                var node = bvhTree[nodeNum];
                float t1 = 0f, u = 0f, v = 0f;
                if (BBox.IntersectBox(node.Bound, ray))
                {
                    if (node.PrimitiveCount > 0)
                    {
                        for (var i = 0; i < node.PrimitiveCount; ++i)
                        {
                            if (!triangles[node.PrimOffset + i].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                                continue;
                            hit = true;
                            break;
                        }
                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else
                    {
                        if (dirIsNeg[node.Axis])
                        {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = node.SecondChildOffset;
                        }
                        else
                        {
                            todo[todoOffset++] = node.SecondChildOffset;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else
                {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            return hit;
        }
        #endregion

        #region IAccellerationStructure Members

        public void Init(SceneGeometryInfo scene, IMaterialProvider materialProvider, TriangleMesh[] geometry)
        {
            var sceneModel = scene;
            meshes = new List<TriangleMesh>();
            meshes.AddRange(geometry);
            this.sceneVertices = sceneModel.Vertices;
            AABB sceneBound = sceneVertices.Select(i => new AABB(i)).Aggregate((b1, b2) => b1.Union(b2));
            var ltriangles = meshes.SelectMany(item => item.triangles).ToList();
            Comparison<Triangle> tc = (t1, t2) => CompareTriangles(t1, t2,
                (Axis)sceneBound.MaximumExtent()
                //                Axis.PLANE_Z
                );
            ltriangles.Sort(tc);


            this.materialProvider = materialProvider;
            Console.WriteLine("Building tree ");
            this.BuildTree(ltriangles, 0, ltriangles.Count);
            Console.WriteLine("BVH builded {0} nodes {1} object in leaf", bvhNodesCount, PrimitivesInNode);
            Console.WriteLine("Flattening tree...");

            this.triangles = ltriangles.ToArray();
            FlattenTree();
            Console.WriteLine("Complete");
        }

        public int CompareTriangles(Triangle t1, Triangle t2, Axis a = Axis.PLANE_Z)
        {
            var p1 = t1.Center(ref sceneVertices);
            var p2 = t2.Center(ref sceneVertices);
            return p1.Component(a).CompareTo(p2.Component(a));
        }

        public bool Intersect(RayInfo ray)
        {
            return ShadowIntersectBVH(ray);
            //return IntersectNode(RootNode, ray);

        }



        public bool Intersect(RayInfo ray, out IntersectionInfo intersection)
        {
            intersection = new IntersectionInfo();
#if VERBOSE
                    try {
#endif
       
                return IntersectBVH(ray, ref intersection);
#if VERBOSE

            }
            catch (Exception ex)
            {
                Console.WriteLine();
                Console.WriteLine(ex.Message);
                Console.WriteLine(ex.StackTrace);
                Console.WriteLine();


            }

            return false;
#endif
            //return IntersectNode(RootNode, ray, out intersection);
        }
/*
        private bool IntersectNode(Node node, RayInfo ray, out IntersectionInfo intersection)
        {
            if (node == null)
            {
                intersection = null;
                return false;
            }
            float t0, t1 = 0f, dist = float.MaxValue, u = 0f, v = 0f;
            if (IntersectBox(node.Bound, ray))
            {
                //ray.MinT = t0;
                //ray.MaxT = t1;

                if (node.nPrims > 0)
                {
                    for (var np = node.StartOffset; np < (node.StartOffset + node.nPrims); np++)
                    {
                        if (triangles[np].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v) && dist > t1)
                        {
                            var triangleMesh = triangles[np].Owner;
                            intersection = new IntersectionInfo();
                            dist = t1;
                            intersection.Distance = dist;
                            intersection.Geometry = triangles[np];
                            intersection.Point = ray.Point(dist);
                            intersection.Material = materialProvider.Get(triangleMesh.MaterialName);

                            var gnormal =
                                intersection.Geometry.InterpolateMeshNormal(u, v).Normalize();
                            //InterpolateNormal(ref sceneVertices, u, v).Normalize();
                            //triangleMesh.GetTriangleNormal(i, 0);
                            //triangleMesh.InterpolateTriangleNormal(i, u, v).Normalize();
                            Normal shadeN;

                            if (Vector.Dot(ref ray.Dir, ref gnormal) >= 0f)
                                shadeN = -gnormal;
                            else
                                shadeN = gnormal;

                            intersection.ShadingNormal = shadeN;
                            intersection.Normal = gnormal;
                            //                                    triangleMesh.InterpolateTriangleNormal(i, u, v);
                            //triangleMesh.triangles[i].InterpolateNormal(ref scene.Vertices, u, v).Normalize();
                            //triangleMesh.triangles[i].InterpolateNormal(ref scene.Vertices, u, v).Normalize();
                            //triangleMesh.triangles[i].ComputeNormal(ref scene.Vertices).Normalize();

                            intersection.TexCoords = new UV(u, v);
                            //triangleMesh.InterpolateTriUV(i, u, v);
                            return true;
                        }
                    }

                }

                IntersectionInfo li, ri;
                var leftIntersect = IntersectNode(node.Left, ray, out li);
                var rIntersect = IntersectNode(node.Right, ray, out ri);
                if (leftIntersect & rIntersect)
                {
                    intersection = li.Distance > ri.Distance ? ri : li;
                    return true;
                }
                if (leftIntersect)
                {
                    intersection = li;
                    return true;
                }
                if (rIntersect)
                {
                    intersection = ri;
                    return true;
                }

            }
            intersection = null;
            return false;
        }
        */

        private bool IntersectNode(Node node, RayInfo ray)
        {
            if (node == null)
            {
                return false;
            }
            float t0, t1 = 0f, u = 0f, v = 0f;
            if (IntersectBox(node.Bound, ray))
            {
                //ray.MinT = t0;
                // ray.MaxT = t1;
                if (node.nPrims > 0)
                {
                    for (var np = node.StartOffset; np < (node.StartOffset + node.nPrims); np++)
                    {
                        if (triangles[np].Intersect(ref sceneVertices, ref ray, ref t1, ref u, ref v))
                        {
                            return true;
                        }
                    }
                }
                return IntersectNode(node.Left, ray) || IntersectNode(node.Right, ray);
            }
            return false;
        }


        private void FindBestSplit(List<Triangle> list, int begin, int end, out float splitValue, out int bestAxis)
        {
            splitValue = 0;
            if (end - begin == 2)
            {
                splitValue = (list[begin].WorldBound(ref sceneVertices).Max[0] + list[begin].WorldBound(ref sceneVertices).Min[0] + list[end - 1].WorldBound(ref sceneVertices).Max[0] + list[end - 1].WorldBound(ref sceneVertices).Min[0]) / 2f;
                bestAxis = 0;
            }
            else
            {
                Point mean2 = new Point(), var = new Point();

                for (int i = begin; i < end; i++)
                    mean2 += list[i].WorldBound(ref sceneVertices).Max + list[i].WorldBound(ref sceneVertices).Min;
                mean2 /= end - begin;

                for (int i = begin; i < end; i++)
                {
                    Vector v = list[i].WorldBound(ref sceneVertices).Max + list[i].WorldBound(ref sceneVertices).Min - mean2;
                    v.x *= v.x;
                    v.y *= v.y;
                    v.z *= v.z;
                    var += v;
                }
                // Select axis with more variance
                if (var.x > var.y && var.x > var.z)
                    bestAxis = 0;
                else if (var.y > var.z)
                    bestAxis = 1;
                else
                    bestAxis = 2;

                if (costSamples > 1)
                {
                    AABB nodeBounds = new AABB();//!!!!
                    for (int i = begin; i < end; i++)
                        nodeBounds = nodeBounds.Union(list[i].WorldBound(ref sceneVertices));

                    Vector d = nodeBounds.Max - nodeBounds.Min;
                    float totalSA = (2f * (d.x * d.y + d.y * d.z + d.z * d.x));
                    float invTotalSA = 1f / totalSA;

                    // Sample cost for split at some points
                    float increment = 2f * d[bestAxis] / (costSamples + 1);
                    float bestCost = float.PositiveInfinity;
                    for (float splitVal = 2f * nodeBounds.Min[bestAxis] + increment; splitVal < 2f * nodeBounds.Max[bestAxis]; splitVal += increment)
                    {
                        int nBelow = 0, nAbove = 0;
                        AABB bbBelow = new AABB(Point.Zero), bbAbove = new AABB(Point.Zero);
                        for (int j = begin; j < end; j++)
                        {
                            if ((list[j].WorldBound(ref sceneVertices).Max[bestAxis] + list[j].WorldBound(ref sceneVertices).Min[bestAxis]) < splitVal)
                            {
                                nBelow++;
                                bbBelow = bbBelow.Union(list[j].WorldBound(ref sceneVertices));
                            }
                            else
                            {
                                nAbove++;
                                bbAbove = bbAbove.Union(list[j].WorldBound(ref sceneVertices));
                            }
                        }
                        Vector dBelow = bbBelow.Max - bbBelow.Min;
                        Vector dAbove = bbAbove.Max - bbAbove.Min;
                        float belowSA = 2f * ((dBelow.x * dBelow.y + dBelow.x * dBelow.z + dBelow.y * dBelow.z));
                        float aboveSA = 2f * ((dAbove.x * dAbove.y + dAbove.x * dAbove.z + dAbove.y * dAbove.z));
                        float pBelow = belowSA * invTotalSA;
                        float pAbove = aboveSA * invTotalSA;
                        float eb = (nAbove == 0 || nBelow == 0) ? emptyBonus : 0;
                        float cost = traversalCost + isectCost * (1f - eb) * (pBelow * nBelow + pAbove * nAbove);
                        // Update best split if this is lowest cost so far
                        if (cost < bestCost)
                        {
                            bestCost = cost;
                            splitValue = splitVal;
                        }
                    }
                }
                else
                {
                    // Split in half around the mean center
                    splitValue = mean2[bestAxis];
                }
            }
        }
        #endregion


        public static bool IntersectBox(AABB box, RayInfo ray)
        {
            float t0 = ray.MinT;
            float t1 = ray.MaxT;
            //float hitt0 = t0;
            //float hitt1 = t1;

            if (!process_box_coord(ref t0, ref t1, box.Min.x - ray.Org.x, box.Max.x - ray.Org.x, ray.InvDir.x))
                return false;

            if (!process_box_coord(ref t0, ref t1, box.Min.y - ray.Org.y, box.Max.y - ray.Org.y, ray.InvDir.y))
                return false;

            if (!process_box_coord(ref t0, ref t1, box.Min.z - ray.Org.z, box.Max.z - ray.Org.z, ray.InvDir.z))
                return false;

            /*
            for (int i = 0; i < 3; ++i) {
                // Update interval for _i_th bounding box slab
                float invRayDir = 1f / ray.Direction[i];
                if (!process_box_coord(ref t0, ref t1, min[i] - ray.Origin[i], max[i] - ray.Origin[i], invRayDir)) return false;
            }
             * */
            //ray.MinT = t0;
            //ray.MaxT = t1;
            //hitt0 = t0;
            //hitt1 = t1;
            return true;
        }
        private static bool process_box_coord(ref float t0, ref float t1, float mnRO, float mxRO, float invRayDir)
        {
            float tNear = (mnRO) * invRayDir;
            float tFar = (mxRO) * invRayDir;
            // Update parametric interval from slab intersection $t$s
            if (tNear > tFar) swap(ref tNear, ref tFar);
            t0 = tNear > t0 ? tNear : t0;
            t1 = tFar < t1 ? tFar : t1;
            if (t0 > t1) return false;
            return true;
        }

        private static void swap(ref float tNear, ref  float tFar)
        {
            float tmp = tNear;
            tNear = tFar;
            tFar = tmp;
        }
    }
}
